#include "bno08x.h"

/********************静态变量定义********************/
static const uint8_t bno_address = BNO085_DEFAULT_ADDRESS; // imu地址
static int8_t bno_status = IMU_NOT_READY;                  // bno状态
static bool sh2_receive_status = false;

/*SH2*/
static uint8_t shtpHeader[4];             // 报文头
static uint8_t shtpData[MAX_PACKET_SIZE]; // 报文内容
static uint8_t sequenceNumber[6] = {0};   // 六个频道的信号量(这个频道报告的次数)
static uint8_t commandSequenceNumber = 0; // 命令的信号量(命令报告的次数)
// static uint32_t metaData[MAX_METADATA_SIZE];  // metadata数据的长度是大于10个word的，但是我们会在读取完“Q point 3”之后停止读取这个metaData数据

/*数据*/
static uint8_t i2c_tmp[I2C_BUFFER_LENGTH] = {0};
// 这些原始值会根据user requested Input Report报文内容更新
//  These are the raw sensor values pulled from the user requested Input Report
static int16_t rawAccelX, rawAccelY, rawAccelZ, accelAccuracy;
static int16_t rawLinAccelX, rawLinAccelY, rawLinAccelZ, accelLinAccuracy;
static int16_t rawGyroX, rawGyroY, rawGyroZ, gyroAccuracy;
static int16_t rawMagX, rawMagY, rawMagZ, magAccuracy;
static int16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy;
static int16_t stepCount;
static uint8_t calibrationStatus = 1; // R0 - Status (0 = success, non-zero = fail)
static unsigned char stabilityClassifier;
static unsigned char activityClassifier;
static unsigned char *_activityConfidences; // Array that store the confidences of the 9 possible activities

/********************静态函数声明********************/

static int8_t bno08x_sh2_send_packet(uint8_t channelNumber, uint16_t dataLength);
static int8_t bno08x_sh2_receive_packet(void);
static bool bno08x_sh2_is_receive_packet(void);
static float bno08x_q_to_float(int fixedPointValue, unsigned char qPoint);
static int8_t bno08x_set_feature_command(unsigned char reportID, unsigned int timeBetweenReports, unsigned long specificConfig);
static int8_t bno08x_set_calibrate_command(unsigned int thingToCalibrate);
static void bno08x_parse_input_report(void);

/********************全局函数定义********************/

/**
 * @brief 初始化BNO08x
 * @return imu/i2c状态码,其中i2c的状态码见 @ref user_i2c.h
 */
int8_t bno08x_init(void)
{
  shtpData[0] = 1; // Reset

  // Attempt to start communication with sensor
  /*复位*/
  bno_status = bno08x_sh2_send_packet(CHANNEL_EXECUTABLE, 1);

  if (bno_status >= IMU_OK) // Transmit packet on channel 1, 1 byte
  {
    delay_ms(200); // 等待复位完毕
    /*复位*/
    for (uint8_t i = 0; i < 2; i++)
    {
      do
      {
        bno_status = bno08x_sh2_receive_packet();
        if (bno_status == IMU_INIT_ERROR || bno_status < IMU_OK)
        {
          return bno_status;
        }
        delay_ms(50);
      } while (bno08x_sh2_is_receive_packet() == true);
    }

    /*设置参数*/
    delay_ms(100);
    for (uint8_t i = 0; i < 4; i++)
    {
      /*第一次设置参数*/
      switch (i)
      {
      case 0:
        // bno_status=bno08x_set_feature_command(SENSOR_REPORTID_LINEAR_ACCELERATION, 3, 0);
        break;
      case 1:
        // bno_status=bno08x_set_feature_command(SENSOR_REPORTID_GYROSCOPE, 10, 0);
        break;
      case 2:
        bno_status = bno08x_set_calibrate_command(CALIBRATE_GYRO);
        delay_ms(20);
        bno_status = bno08x_sh2_receive_packet();
        bno08x_parse_input_report();
        break;
      case 3:
        bno_status = bno08x_set_feature_command(SENSOR_REPORTID_ARVR_STABILIZED_GRV, 1, 0);
        break;
      }
      /*如果发送状态不正常，重试3次*/
      if (bno_status != I2C_OK || (i == 2 && calibrationStatus == 1))
      {
        for (uint8_t j = 0; j < 3; i++)
        {
          switch (i)
          {
          case 0:
            // bno_status=bno08x_set_feature_command(SENSOR_REPORTID_LINEAR_ACCELERATION, 3, 0);
            break;
          case 1:
            // bno_status=bno08x_set_feature_command(SENSOR_REPORTID_GYROSCOPE, 10, 0);
            break;
          case 2:
            bno_status = bno08x_set_calibrate_command(CALIBRATE_GYRO);
            delay_ms(20);
            bno_status = bno08x_sh2_receive_packet();
            bno08x_parse_input_report();
            break;
          case 3:
            bno_status = bno08x_set_feature_command(SENSOR_REPORTID_ARVR_STABILIZED_GRV, 1, 0);
            break;
          }
          if ((i != 2 && bno_status == I2C_OK) || (i == 2 && calibrationStatus == 0))
            break;
        }
      }
      /*如果重试失败，返回状态码*/
      if (bno_status != I2C_OK)
        return bno_status;
    }
  }

  return bno_status;
}

/**
 * @brief 更新BNO08x四元数数据
 * @param qr 四元数实部指针
 * @param qi 四元数虚部i指针
 * @param qj 四元数虚部j指针
 * @param qk 四元数虚部k指针
 * @return imu/i2c状态码,其中i2c的状态码见 @ref user_i2c.h
 */
int8_t bno08x_read_all(float *qr, float *qi, float *qj, float *qk)
{
  /*检查IMU状态*/
  if (bno_status == IMU_NOT_READY || bno_status == IMU_INIT_ERROR || bno_status < IMU_OK)
    return bno_status;

  /*发起一次读取*/
  bno_status = bno08x_sh2_receive_packet();

  bno_status = IMU_NO_DATA_INPUT;
  /*如果接收到了数据包*/
  if (bno08x_sh2_is_receive_packet() == true)
  {
    // Check to see if this packet is a sensor reporting its data to us
    if (shtpHeader[2] == CHANNEL_REPORTS && shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP)
    {
      bno_status = IMU_OK;
      /*解包，更新参数*/
      bno08x_parse_input_report();
      /*Q格式转float，更新四元数*/
      *qr = bno08x_q_to_float(rawQuatReal, (unsigned char)ROTATION_VECTOR_Q1);
      *qi = bno08x_q_to_float(rawQuatI, (unsigned char)ROTATION_VECTOR_Q1);
      *qj = bno08x_q_to_float(rawQuatJ, (unsigned char)ROTATION_VECTOR_Q1);
      *qk = bno08x_q_to_float(rawQuatK, (unsigned char)ROTATION_VECTOR_Q1);
    }
  }

  return bno_status;
}

/********************静态函数定义********************/

/**
 * @brief SH2发送数据包
 * @return SH2-I2C状态码
 */
static int8_t bno08x_sh2_send_packet(uint8_t channelNumber, uint16_t dataLength)
{
  const uint16_t packetLength = dataLength + 4; // Add four bytes for the header
  int8_t status;

  /*检查数据大小*/
  if (packetLength > I2C_BUFFER_LENGTH)
    return (IMU_DATA_LENGTH_OUT_OF_RANGE);

  /*设置报文头*/
  i2c_tmp[0] = (packetLength & 0xFF);             // Packet length LSB
  i2c_tmp[1] = (0);                               // Packet length MSB
  i2c_tmp[2] = (channelNumber);                   // Channel number
  i2c_tmp[3] = (sequenceNumber[channelNumber]++); // Send the sequence number, increments with each packet sent, different counter for each channel

  /*设置报文内容*/
  for (uint8_t i = 0; i < dataLength; i++)
  {
    i2c_tmp[4 + i] = (shtpData[i]);
  }
  status = i2c_write(IMU_I2C_INST, bno_address, I2C_REG_ADDR_NONE, i2c_tmp, packetLength);
  return status;
}

/**
 * @brief SH2接收数据包
 * @return SH2-I2C状态码
 * @note 会更新  sh2_receive_status 来表示是否接收到数据，
 * 用 @ref bno08x_sh2_is_receive_packet() 函数来检查这个变量
 */
static int8_t bno08x_sh2_receive_packet(void)
{
  uint32_t dataLength = 0;
  uint8_t packetLSB = 0;
  uint8_t packetMSB = 0;
  uint8_t channelNumber = 0;
  uint8_t sequenceNumber = 0;
  int8_t status;

  /*获取报文头信息*/
  status = i2c_read(IMU_I2C_INST, bno_address, I2C_REG_ADDR_NONE, i2c_tmp, 4);
  if (bno_status != IMU_NOT_READY && bno_status != IMU_INIT_ERROR && bno_status >= IMU_OK)
  {
    shtpHeader[0] = packetLSB = i2c_tmp[0];
    shtpHeader[1] = packetMSB = i2c_tmp[1];
    shtpHeader[2] = channelNumber = i2c_tmp[2];
    shtpHeader[3] = sequenceNumber = i2c_tmp[3]; // Not sure if we need to store this or not

    /*计算数据包的字节数*/
    dataLength = ((unsigned int)packetMSB << 8 | packetLSB);
    dataLength &= ~(1 << 15); // 清除最高比特位。

    /* 此位（dataLength == 0?）指示此包是否是上一个包的延续。暂时忽略它。
     * TODO 将此捕获为错误并退出
     */
    if (dataLength == 0)
    {
      return status;
    }
    if (dataLength > 4)
    {
      if (dataLength > MAX_PACKET_SIZE)
        dataLength = MAX_PACKET_SIZE;
      /*获取报文内容*/
      status = i2c_read(IMU_I2C_INST, bno_address, I2C_REG_ADDR_NONE, i2c_tmp, dataLength);
      if (bno_status != IMU_NOT_READY && bno_status != IMU_INIT_ERROR && bno_status >= IMU_OK)
      {
        for (uint16_t x = 0; x < dataLength - 4; x++)
        {
          if (x < MAX_PACKET_SIZE)
          {
            shtpData[x] = i2c_tmp[4 + x]; // Store data into the shtpData array
          }
        }
      }
    }
    if (bno_status != IMU_NOT_READY && bno_status != IMU_INIT_ERROR && bno_status >= IMU_OK)
      sh2_receive_status = true;
  }

  return status;
}

/**
 * @brief SH2检查接收状态
 * @return 接收状态
 */
static bool bno08x_sh2_is_receive_packet(void)
{
  bool status = sh2_receive_status;

  sh2_receive_status = false;

  return status;
}

/*Q格式转float*/
// Given a register value and a Q point, convert to float
// See https://en.wikipedia.org/wiki/Q_(number_format)
static float bno08x_q_to_float(int fixedPointValue, unsigned char qPoint)
{
  float qFloat = fixedPointValue;
  qFloat *= 1.0f / (1 << qPoint); // 2的qPoint * -1次方
  return (qFloat);
}

/**
 * @brief 设置feature_command命令
 * @param reportID 要设置的报告ID，可设置的报告见 @ref bno085.h->SENSOR_REPORTID 开头的宏定义
 * @param timeBetweenReports 报告间隔，单位ms,设为0即表示后续读取时不返回这个报告
 * @param specificConfig 特殊设置，一般设为 0
 * @return imu/i2c状态码
 * @note 用于启用、设置某个报告
 */
static int8_t bno08x_set_feature_command(unsigned char reportID, unsigned int timeBetweenReports, unsigned long specificConfig)
{
  long microsBetweenReports = (long)timeBetweenReports * 1000L;
  shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND;     // Set feature command. Reference page 55
  shtpData[1] = reportID;                            // Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
  shtpData[2] = 0;                                   // Feature flags
  shtpData[3] = 0;                                   // Change sensitivity (LSB)
  shtpData[4] = 0;                                   // Change sensitivity (MSB)
  shtpData[5] = (microsBetweenReports >> 0) & 0xFF;  // Report interval (LSB) in microseconds. 0x7A120 = 500ms
  shtpData[6] = (microsBetweenReports >> 8) & 0xFF;  // Report interval
  shtpData[7] = (microsBetweenReports >> 16) & 0xFF; // Report interval
  shtpData[8] = (microsBetweenReports >> 24) & 0xFF; // Report interval (MSB)
  shtpData[9] = 0;                                   // Batch Interval (LSB)
  shtpData[10] = 0;                                  // Batch Interval
  shtpData[11] = 0;                                  // Batch Interval
  shtpData[12] = 0;                                  // Batch Interval (MSB)
  shtpData[13] = (specificConfig >> 0) & 0xFF;       // Sensor-specific config (LSB)
  shtpData[14] = (specificConfig >> 8) & 0xFF;       // Sensor-specific config
  shtpData[15] = (specificConfig >> 16) & 0xFF;      // Sensor-specific config
  shtpData[16] = (specificConfig >> 24) & 0xFF;      // Sensor-specific config (MSB)

  // Transmit packet on channel 2, 17 bytes
  return bno08x_sh2_send_packet(CHANNEL_CONTROL, 17);
}

/**
 * @brief 设置校准命令
 * @param thingToCalibrate 要校准的传感器，可校准的传感器见 @ref bno085.h->CALIBRATE 开头的宏定义
 * @return imu/i2c状态码
 * @note 用于校准设置命令
 */
static int8_t bno08x_set_calibrate_command(unsigned int thingToCalibrate)
{
  shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; // Command Request
  shtpData[1] = commandSequenceNumber++;     // Increments automatically each function call
  shtpData[2] = COMMAND_ME_CALIBRATE;        // Feature flags
  for (uint8_t i = 3; i < 12; i++)
  {
    shtpData[i] = 0;
  }
  if (thingToCalibrate == CALIBRATE_ACCEL)
    shtpData[3] = 1;
  else if (thingToCalibrate == CALIBRATE_GYRO)
    shtpData[4] = 1;
  else if (thingToCalibrate == CALIBRATE_MAG)
    shtpData[5] = 1;
  else if (thingToCalibrate == CALIBRATE_PLANAR_ACCEL)
    shtpData[7] = 1;
  else if (thingToCalibrate == CALIBRATE_ACCEL_GYRO_MAG)
  {
    shtpData[3] = 1;
    shtpData[4] = 1;
    shtpData[5] = 1;
  }
  else if (thingToCalibrate == CALIBRATE_STOP)
  {
    ; // Do nothing, bytes were set to zero
  }

  calibrationStatus = 1;
  // Transmit packet on channel 2, 17 bytes
  return bno08x_sh2_send_packet(CHANNEL_CONTROL, 12);
}

/**
 * @brief 解析bno08x数据包
 * @note This function pulls the data from the input report.
 * The input reports vary in length so this function stores the various 16-bit values as globals
 * Unit responds with packet that contains the following:
 * shtpHeader[0:3]: First, a 4 byte header
 * shtpData[0:4]: Then a 5 byte timestamp of microsecond clicks since reading was taken
 * shtpData[5 + 0]: Then a feature report ID (0x01 for Accel, 0x05 for Rotation Vector)
 * shtpData[5 + 1]: Sequence number (See 6.5.18.2)
 * shtpData[5 + 2]: Status
 * shtpData[3]: Delay
 * shtpData[4:5]: i/accel x/gyro x/etc
 * shtpData[6:7]: j/accel y/gyro y/etc
 * shtpData[8:9]: k/accel z/gyro z/etc
 * shtpData[10:11]: real/gyro temp/etc
 * shtpData[12:13]: Accuracy estimate
 */
static void bno08x_parse_input_report(void)
{
  // Calculate the number of data bytes in this packet
  int dataLength = ((unsigned int)shtpHeader[1] << 8 | shtpHeader[0]);

  unsigned char status = shtpData[5 + 2] & 0x03; // Get status bits
  int16_t data1 = (int16_t)shtpData[5 + 5] << 8 | shtpData[5 + 4];
  int16_t data2 = (int16_t)shtpData[5 + 7] << 8 | shtpData[5 + 6];
  int16_t data3 = (int16_t)shtpData[5 + 9] << 8 | shtpData[5 + 8];
  int16_t data4 = 0;
  int16_t data5 = 0;
  unsigned char x = 0;
  dataLength &= ~(1 << 15); // Clear the MSbit. This bit indicates if this package is a continuation of the last.
  // Ignore it for now. TODO catch this as an error and exit

  dataLength -= 4; // Remove the header bytes from the data count

  if (dataLength - 5 > 9)
  {
    data4 = (int16_t)shtpData[5 + 11] << 8 | shtpData[5 + 10];
  }
  if (dataLength - 5 > 11)
  {
    data5 = (int16_t)shtpData[5 + 13] << 8 | shtpData[5 + 12];
  }

  // Store these generic values to their proper global variable
  if (shtpData[5] == SENSOR_REPORTID_ACCELEROMETER)
  {
    accelAccuracy = status;
    rawAccelX = data1;
    rawAccelY = data2;
    rawAccelZ = data3;
  }
  else if (shtpData[5] == SENSOR_REPORTID_LINEAR_ACCELERATION)
  {
    accelLinAccuracy = status;
    rawLinAccelX = data1;
    rawLinAccelY = data2;
    rawLinAccelZ = data3;
  }
  else if (shtpData[5] == SENSOR_REPORTID_GYROSCOPE)
  {
    gyroAccuracy = status;
    rawGyroX = data1;
    rawGyroY = data2;
    rawGyroZ = data3;
  }
  else if (shtpData[5] == SENSOR_REPORTID_MAGNETIC_FIELD)
  {
    magAccuracy = status;
    rawMagX = data1;
    rawMagY = data2;
    rawMagZ = data3;
  }
  else if (shtpData[5] == SENSOR_REPORTID_ROTATION_VECTOR || shtpData[5] == SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR ||
           shtpData[5] == SENSOR_REPORTID_GAME_ROTATION_VECTOR || shtpData[5] == SENSOR_REPORTID_ARVR_STABILIZED_RV ||
           shtpData[5] == SENSOR_REPORTID_ARVR_STABILIZED_GRV)
  {
    quatAccuracy = status;
    rawQuatI = data1;
    rawQuatJ = data2;
    rawQuatK = data3;
    rawQuatReal = data4;
    rawQuatRadianAccuracy = data5; // Only available on rotation vector, not game rot vector
  }
  else if (shtpData[5] == SENSOR_REPORTID_STEP_COUNTER)
  {
    stepCount = data3; // Bytes 8/9
  }
  else if (shtpData[5] == SENSOR_REPORTID_STABILITY_CLASSIFIER)
  {
    stabilityClassifier = shtpData[5 + 4]; // Byte 4 only
  }
  else if (shtpData[5] == SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER)
  {
    activityClassifier = shtpData[5 + 5]; // Most likely state

    // Load activity classification confidences into the array
    for (x = 0; x < 9; x++)                          // Hardcoded to max of 9. TODO - bring in array size
      _activityConfidences[x] = shtpData[5 + 6 + x]; // 5 bytes of timestamp, byte 6 is first confidence byte
  }
  else if (shtpData[0] == SHTP_REPORT_COMMAND_RESPONSE)
  {
    // The BNO080 responds with this report to command requests. It's up to use to remember which command we issued.
    uint8_t command = shtpData[0 + 2]; // This is the Command byte of the response

    if (command == COMMAND_ME_CALIBRATE)
    {
      calibrationStatus = shtpData[0 + 5]; // R0 - Status (0 = success, non-zero = fail)
    }
  }
  else
  {
    // This sensor report ID is unhandled.
    // See reference manual to add additional feature reports as needed
  }

  // TODO additional feature reports may be strung together. Parse them all.
}